﻿/* RDCorrection.cpp
 * 
 * Copyright (C) 2020, 2021 Yuan LI
 * email:18069211@qq.com
 * https://blog.csdn.net/liyuanbhu
 * 
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or (at
 * your option) any later version.
 * 
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */


#include "RDCorrection.h"

RDCorrection::RDCorrection()
{
    m_bg_rgb32 = qRgb(0, 0, 0);
    m_bg_gray = 0;
}
QImage RDCorrection::map_bilinear(const QImage &in, double k)
{
    if(in.isNull())
    {
        return QImage();
    }
    switch (in.format())
    {
    case QImage::Format_RGB32:
    case QImage::Format_ARGB32:
    case QImage::Format_ARGB32_Premultiplied:
        return map_rgb32_bilinear(in, k);
        break;

    case QImage::Format_Grayscale8:
    case QImage::Format_Indexed8:
        return map_gray_bilinear(in, k);
        break;
    default:
        break;
    }
    return QImage();
}

QImage RDCorrection::map_nearest(const QImage &in, double k)
{
    if(in.isNull())
    {
        return QImage();
    }
    switch (in.format())
    {
    case QImage::Format_RGB32:
    case QImage::Format_ARGB32:
    case QImage::Format_ARGB32_Premultiplied:
        return map_rgb32_nearest(in, k);
        break;

    case QImage::Format_Grayscale8:
    case QImage::Format_Indexed8:
        return map_gray_nearest(in, k);
        break;
    default:
        break;
    }
    return QImage();
}

bool RDCorrection::map_nearest(const QImage &in, double k, QImage &out)
{
    if(in.isNull() || out.isNull())
    {
        return false;
    }
    switch (in.format())
    {
    case QImage::Format_RGB32:
    case QImage::Format_ARGB32:
    case QImage::Format_ARGB32_Premultiplied:
        return map_rgb32_nearest(in, k, out);
        break;

    case QImage::Format_Grayscale8:
    case QImage::Format_Indexed8:
        return map_gray_nearest(in, k, out);
        break;
    default:
        break;
    }
    return false;
}

bool RDCorrection::map_bilinear(const QImage &in, double k, QImage &out)
{
    if(in.isNull() || out.isNull())
    {
        return false;
    }
    switch (in.format())
    {
    case QImage::Format_RGB32:
    case QImage::Format_ARGB32:
    case QImage::Format_ARGB32_Premultiplied:
        return map_rgb32_bilinear(in, k, out);
        break;

    case QImage::Format_Grayscale8:
    case QImage::Format_Indexed8:
        return map_gray_bilinear(in, k, out);
        break;
    default:
        break;
    }
    return false;
}

bool RDCorrection::map_rgb32_bilinear(const QImage &in, double k, QImage &out)
{
    if(out.isNull() || in.isNull() || out.format() != in.format())
    {
        return false;
    }
    int cols_out = out.width();
    int rows_out = out.height();
    int cols_in = in.width();
    int rows_in = in.height();

    double xc_out = cols_out / 2.0;
    double yc_out = rows_out / 2.0;
    double xc_in = cols_in / 2.0;
    double yc_in = rows_in / 2.0;

    const uchar * source = in.bits();
    int lineByte = in.bytesPerLine();
    for(int y = 0; y < rows_out; y ++)
    {
        QRgb * dest = (QRgb * )out.scanLine(y);

        for(int x = 0; x < cols_out; x ++)
        {

            double xx = (x - xc_out) / 1000.0;
            double yy = (y - yc_out) / 1000.0;
            double scale = 1 / (1 + k * (xx * xx + yy * yy));
            xx = xx * scale * 1000.0 + xc_in;
            yy = yy * scale * 1000.0 + yc_in;
            int ix = xx;
            int iy = yy;
            if(iy < 0 || iy >= rows_in || ix < 0 || ix >= cols_in)
            {
                dest[x] = m_bg_rgb32;
            }
            else
            {
                QRgb * line0 = (QRgb *) (source + iy * lineByte);
                QRgb * line1 = (QRgb *) (source + iy * lineByte + lineByte);
                dest[x] = bilinear(line0[ix], line0[ix + 1], line1[ix], line1[ix + 1], xx - ix, yy - iy);
            }
        }
    }
    return true;
}

bool RDCorrection::map_rgb32_nearest(const QImage &in, double k, QImage &out)
{
    if(out.isNull() || in.isNull() || out.format() != in.format())
    {
        return false;
    }
    int cols_out = out.width();
    int rows_out = out.height();
    int cols_in = in.width();
    int rows_in = in.height();

    double xc_out = cols_out / 2.0;
    double yc_out = rows_out / 2.0;
    double xc_in = cols_in / 2.0;
    double yc_in = rows_in / 2.0;

    const uchar * source = in.bits();
    int lineByte = in.bytesPerLine();
    for(int y = 0; y < rows_out; y ++)
    {
        QRgb * dest = (QRgb * )out.scanLine(y);

        for(int x = 0; x < cols_out; x ++)
        {

            double xx = (x - xc_out) / 1000.0;
            double yy = (y - yc_out) / 1000.0;
            double scale = 1 / (1 + k * (xx * xx + yy * yy));
            xx = xx * scale * 1000.0 + xc_in;
            yy = yy * scale * 1000.0 + yc_in;
            int ix = xx;
            int iy = yy;
            if(iy < 0 || iy >= rows_in || ix < 0 || ix >= cols_in)
            {
                dest[x] = m_bg_rgb32;
            }
            else
            {
                QRgb * line = (QRgb *) (source + iy * lineByte);
                dest[x] = line[ix];
            }
        }
    }
    return true;
}

bool RDCorrection::map_gray_nearest(const QImage &in, double k, QImage &out)
{
    if(out.isNull() || in.isNull() || out.format() != in.format())
    {
        return false;
    }
    int cols_out = out.width();
    int rows_out = out.height();
    int cols_in = in.width();
    int rows_in = in.height();

    double xc_out = cols_out / 2.0;
    double yc_out = rows_out / 2.0;
    double xc_in = cols_in / 2.0;
    double yc_in = rows_in / 2.0;

    const uchar * source = in.bits();
    int lineByte = in.bytesPerLine();
    for(int y = 0; y < rows_out; y ++)
    {
        uchar * dest = (uchar * )out.scanLine(y);

        for(int x = 0; x < cols_out; x ++)
        {

            double xx = (x - xc_out) / 1000.0;
            double yy = (y - yc_out) / 1000.0;
            double scale = 1 / (1 + k * (xx * xx + yy * yy));
            xx = xx * scale * 1000.0 + xc_in;
            yy = yy * scale * 1000.0 + yc_in;
            int ix = xx;
            int iy = yy;
            if(iy < 0 || iy >= rows_in || ix < 0 || ix >= cols_in)
            {
                dest[x] = m_bg_gray;
            }
            else
            {
                uchar * line0 = (uchar *) (source + iy * lineByte);
                uchar * line1 = (uchar *) (source + iy * lineByte + lineByte);
                dest[x] = bilinear(line0[ix], line0[ix + 1], line1[ix], line1[ix + 1], xx - ix, yy - iy);
            }
        }
    }
    return true;
}

bool RDCorrection::map_gray_bilinear(const QImage &in, double k, QImage &out)
{
    if(out.isNull() || in.isNull() || out.format() != in.format())
    {
        return false;
    }
    int cols_out = out.width();
    int rows_out = out.height();
    int cols_in = in.width();
    int rows_in = in.height();

    double xc_out = cols_out / 2.0;
    double yc_out = rows_out / 2.0;
    double xc_in = cols_in / 2.0;
    double yc_in = rows_in / 2.0;

    const uchar * source = in.bits();
    int lineByte = in.bytesPerLine();
    for(int y = 0; y < rows_out; y ++)
    {
        uchar * dest = (uchar * )out.scanLine(y);

        for(int x = 0; x < cols_out; x ++)
        {

            double xx = (x - xc_out) / 1000.0;
            double yy = (y - yc_out) / 1000.0;
            double scale = 1 / (1 + k * (xx * xx + yy * yy));
            xx = xx * scale * 1000.0 + xc_in;
            yy = yy * scale * 1000.0 + yc_in;
            int ix = xx;
            int iy = yy;
            if(iy < 0 || iy >= rows_in || ix < 0 || ix >= cols_in)
            {
                dest[x] = m_bg_gray;
            }
            else
            {
                uchar * line = (uchar *) (source + iy * lineByte);
                dest[x] = line[ix];
            }
        }
    }
    return true;
}

QImage RDCorrection::map_gray_nearest(const QImage &in, double k)
{
    QImage out(in.width(), in.height(), in.format());
    map_rgb32_nearest(in, k, out);
    return out;
}

QImage RDCorrection::map_gray_bilinear(const QImage &in, double k)
{
    QImage out(in.width(), in.height(), in.format());
    map_gray_bilinear(in, k, out);
    return out;
}

QImage RDCorrection::map_rgb32_nearest(const QImage &in, double k)
{
    QImage out(in.width(), in.height(), in.format());
    map_gray_nearest(in, k, out);
    return out;
}

QImage RDCorrection::map_rgb32_bilinear(const QImage &in, double k)
{
    QImage out(in.width(), in.height(), in.format());
    map_rgb32_bilinear(in, k, out);
    return out;
}
